﻿using System;
using System.Threading;
using Microsoft.SPOT;
using GT = Gadgeteer;
using Gadgeteer.Modules.Velloso;
using Gadgeteer.Modules.GHIElectronics;

namespace Coding4Fun.BluetoothPanoHead
{
    /// <summary>
    /// This method is run when the mainboard is powered up or reset.
    /// </summary>
    public partial class Program
    {
        // Create an instance of the Bluetooth class and assign it to socket 6   
        readonly Bluetooth _bluetooth = new Bluetooth(6);

        // A reference to the Bluetooth member that lets us sends text back.
        Bluetooth.Client _client;

        // Create an instance of the shutter control class and assign it to socket 5
        readonly ShutterControl _shutterControl = new ShutterControl(5);

        ServoControl _servoControl;

        readonly CommandParser _commandParser = new CommandParser();
        readonly JoyStickCommand _joyStickCommand = new JoyStickCommand();

        private Boolean _inPairingMode = false;
        private Boolean _haltRequested = false;

        void ProgramStarted()
        {
            _servoControl = new ServoControl(extender, GT.Socket.Pin.Seven);

            joystick.JoystickReleased += JoystickJoystickReleased;

            InitBluetooth();

            var t = new Thread(CommsThread);

            t.Start();

            // Use the blinkenlichten to let the user that the pano head is ready for commands
            SpinLights();

            Debug.Print("Program Started");
        }

        /// <summary>
        /// Set up the handlers for the Bluetooth module
        /// </summary>
        void InitBluetooth()
        {
            // Keep a quick reference to the Bluetooth client, to make it
            // easier to send commands back over the module
            _client = _bluetooth.ClientMode;

            // Set the Bluetooth device name to make it easier to identify when
            // you pair this module with your phon
            _bluetooth.SetDeviceName("Coding4Fun Pano Head");

            // We want to track when we are connected and disconnected
            _bluetooth.BluetoothStateChanged += BluetoothBluetoothStateChanged;

            // We want to collect all the data coming in
            _bluetooth.DataReceived += BluetoothDataReceived;
        }

        /// <summary>
        /// Takes the incoming data from the Bluetooth module and sends it to the command parser
        /// </summary>
        /// <param name="sender">The Bluetooth module that sent the data.</param>
        /// <param name="data">string data received by the bluetooth module.</param>
        void BluetoothDataReceived(Bluetooth sender, string data)
        {
            _commandParser.AddString(data);
            Debug.Print("Received: [" + data + "]");
        }

        /// <summary>
        /// Monitors when the bluetooth state changes.  When a new connection is made, it responds back to the caller
        /// </summary>
        /// <param name="sender">The Bluetooth module.</param>
        /// <param name="btState">New state of the Bluetooth module.</param>
        void BluetoothBluetoothStateChanged(Bluetooth sender, Bluetooth.BluetoothState btState)
        {
            Debug.Print("New state:" + btState.ToString());
            if (btState == Bluetooth.BluetoothState.Connected)
            {
                SendStatusMessage("Connected");
            }
        }

        private void SendErrorMessage(string message)
        {
            string msg = "@" + Commands.CMD_ERROR + "," + message + "#";
            Debug.Print(msg);
            _client.Send(msg);
        }

        private void SendStatusMessage(string message)
        {
            var msg = "@" + Commands.CMD_STATUS + "," + message + "#";
            Debug.Print(msg);
            _client.Send(msg);
        }

        /// <summary>
        /// Check for and execute commands queued up in the command parser.
        /// </summary>
        private void CheckForCommands()
        {
            if (_commandParser.Count > 0)
            {
                ControlCommand controlCommand = _commandParser.Dequeue();

                switch (controlCommand.Action)
                {
                    case Commands.CMD_LEDS:
                        SpinLights();
                        SendStatusMessage("OK");
                        break;
                    case Commands.CMD_VERS:
                        SendVersion();
                        break;
                    case Commands.CMD_STOP:
                        _haltRequested = true;
                        _servoControl.Stop();
                        break;
                    case Commands.CMD_RIGHT:
                        SetServoSpeed(controlCommand.Params);
                        _servoControl.Turn(_servoControl.Speed);
                        break;
                    case Commands.CMD_LEFT:
                        SetServoSpeed(controlCommand.Params);
                        _servoControl.Turn(-_servoControl.Speed);
                        break;
                    case Commands.CMD_SHUT:
                        _shutterControl.TakePicture();
                        break;
                    case Commands.CMD_PANO:
                        var p = new PanoSettings();
                        p.Load(controlCommand.Params);
                        PanoramaOperation(p);
                        break;
                    case Commands.CMD_GETSPEED:
                        SendSpeed(_servoControl.Speed);
                        break;
                    case Commands.CMD_SETSPEED:
                        SetServoSpeed(controlCommand.Params);
                        break;
                }
            }
        }

        /// <summary>
        /// If a turn request specified a speed, return that speed value.  Otherwise return the default
        /// </summary>
        /// <param name="param">An aray of string parameters, can be empty</param>
        /// <returns>int value for speed</returns>
        private int GetSpeedRequestOrDefault(string[] param)
        {
            int newSpeed = _servoControl.Speed;

            if (param != null)
            {
                int i = param.Length;

                if (i > 0)
                {
                    newSpeed = Convert.ToInt32(param[0]);
                }
            }

            return newSpeed;
        }

        /// <summary>
        /// Sets the servo speed based on the supplied parameter..
        /// </summary>
        /// <param name="param">An aray of string parameters, can be empty</param>
        private void SetServoSpeed(string[] param)
        {
            _servoControl.Speed = GetSpeedRequestOrDefault(param);
        }

        private void SendSpeed(int speed)
        {
            SendStatusMessage("SPEED=" + speed.ToString());
        }

        /// <summary>
        /// Some commands can be initiated by the joystick
        /// </summary>
        private void CheckJoyStick()
        {
            if (_joyStickCommand.CheckState() == JoyStickCommandState.DoubleClicked)
            {
                var direction = _joyStickCommand.GetDirection(joystick.GetJoystickPosition());

                switch (direction)
                {
                    case JoyStickDirection.Right:
                        _servoControl.Turn(_servoControl.Speed);

                        // turn to the right until the joystick is moved to a different direction
                        while (direction == JoyStickDirection.Right)
                        {
                            Thread.Sleep(300);
                            direction = _joyStickCommand.GetDirection(joystick.GetJoystickPosition());
                        }
                        _servoControl.Stop();
                        break;

                    case JoyStickDirection.Left:
                        _servoControl.Turn(-_servoControl.Speed);

                        // turn to the right until the joystick is moved to a different direction
                        while (direction == JoyStickDirection.Left)
                        {
                            Thread.Sleep(300);
                            direction = _joyStickCommand.GetDirection(joystick.GetJoystickPosition());
                        }
                        _servoControl.Stop();
                        break;

                    case JoyStickDirection.Up:
                        if (!_inPairingMode)
                        {
                            _client.EnterPairingMode();
                        }
                        else
                        {
                            _bluetooth.Reset();
                        }
                        _inPairingMode = !_inPairingMode;
                        break;

                    case JoyStickDirection.Down:
                        _servoControl.Calibrate();

                        while (direction == JoyStickDirection.Down)
                        {
                            Thread.Sleep(300);
                            direction = _joyStickCommand.GetDirection(joystick.GetJoystickPosition());
                        }
                        _servoControl.Stop();
                        break;
                }
                if (direction != JoyStickDirection.None)
                {
                    _joyStickCommand.ClearState();
                }
            }
        }

        /// <summary>
        /// Main I/O processing loop.  Checks for incomming Bluetooth commands and monitors the state of the joystick
        /// </summary>
        public void CommsThread()
        {
            while (true)
            {
                CheckForCommands();
                CheckJoyStick();
                Thread.Sleep(250);
            }
        }

        void SpinLights()
        {
            for (int i = 1; i < 8; i++)
            {
                led7r.TurnLightOn(i, true);
                Thread.Sleep(100);
            }
            led7r.TurnLightOff(7);
        }

        void SendVersion()
        {
            SendStatusMessage("BT Pano Head vers 1.0.0.1");
        }


        /// <summary>
        /// Register a joystick click event
        /// </summary>
        /// <param name="sender">The sender.</param>
        /// <param name="state">The state.</param>
        void JoystickJoystickReleased(Joystick sender, Joystick.JoystickState state)
        {
            _joyStickCommand.Click();
        }

        /// <summary>
        /// Perform the panoramic operations.
        /// Rotate the head, wait, take picture, wait, repeat until done
        /// </summary>
        /// <param name="p">The panoramic settings.</param>
        void PanoramaOperation(PanoSettings p)
        {
            try
            {
                for (var i = 0; i < p.Iterations; i++)
                {
                    if (_haltRequested)
                        break;

                    _servoControl.Turn(_servoControl.Speed);

                    Thread.Sleep(p.Duration);

                    _servoControl.Stop();

                    if (_haltRequested)
                        break;

                    Thread.Sleep(p.Settle);

                    if (_haltRequested)
                        break;

                    _shutterControl.TakePicture(p.Shutter);

                    if (_haltRequested)
                        break;

                    Thread.Sleep(p.Pause);
                }
            }
            catch (Exception ex)
            {
                SendErrorMessage(ex.Message);
            }

            _haltRequested = false;
        }
    }
}
